#include "load.h"
#include "functions.h"
#include "keypoints.h"
#include "correspond.h"
#include "features.h"
#include "registration.h"
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/transformation_validation.h>
#include <pcl/registration/transformation_validation_euclidean.h>

void
initialRegisterSourceToTargetEdmond (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        PointCloud<PointXYZI>::Ptr source_keypoints,
        PointCloud<PointXYZI>::Ptr target_keypoints,
        CorrespondencesPtr correspond,
        Eigen::Matrix4f &initial_transformation_matrix,
        float resolution)
{
    time_t start,end;
    time (&start);


    voxelKeypoints (source, source_keypoints, resolution); voxelKeypoints(target, target_keypoints, resolution);



    //FPFH
    PointCloud<FPFHSignature33>::Ptr source_features (new PointCloud<FPFHSignature33>);
    PointCloud<FPFHSignature33>::Ptr target_features (new PointCloud<FPFHSignature33>);
    fpfhWithUniformKeypoints (source,source_keypoints, source_features);
    fpfhWithUniformKeypoints (target,target_keypoints, target_features);


    findCorrespondencesFPFH(source_features,target_features,correspond);

    double thresh=5;
    float bestScore=FLT_MAX;
    double bestThresh=0;
    CorrespondencesPtr currentCorrespond(new Correspondences);
    CorrespondencesPtr bestCorrespond(new Correspondences);

    //TRANSFORM ESTIMATION
    registration::TransformationEstimationSVD<PointXYZI, PointXYZI>::Ptr transformation_estimation (new registration::TransformationEstimationSVD<PointXYZI, PointXYZI>);

    while(thresh<20)
    {
        *currentCorrespond=*correspond;
        //FILTER CORRESPONDENCES
        filterCorrespondencesSAC(source_keypoints,target_keypoints,currentCorrespond,thresh);
        //cout << "Initial matrix calculation with thresh "<< thresh <<"..." << flush;
        transformation_estimation->estimateRigidTransformation (*source_keypoints, *target_keypoints, *currentCorrespond, initial_transformation_matrix);
        PointCloud<PointXYZI>::Ptr registered_keypoints(new PointCloud<PointXYZI>);
        transformPointCloud(*source_keypoints, *registered_keypoints, initial_transformation_matrix);
        float score=validationScoreProximityTarget<PointXYZI>(*registered_keypoints,*target_keypoints);
        //cout << " score: "<< score <<flush;
        if(score<bestScore)
        {
            bestScore=score;
            bestThresh=thresh;
            *bestCorrespond= *currentCorrespond;
        }
        thresh+=1;
    }
    *correspond=*bestCorrespond;
    cout << "Initial matrix calculation..." << flush;
    transformation_estimation->estimateRigidTransformation (*source_keypoints, *target_keypoints, *correspond, initial_transformation_matrix);
    PointCloud<PointXYZI>::Ptr registered_keypoints(new PointCloud<PointXYZI>);
    transformPointCloud(*source_keypoints, *registered_keypoints, initial_transformation_matrix);
    transformPointCloud(*source, *registered, initial_transformation_matrix);
    cout << "OK! Used "<< correspond->size() <<" corresponcences. Score on keypoints: " << bestScore<<endl;


    time (&end);
    double dif = difftime (end,start);
    cout << "------------------------------------->  Initial registration in " << dif << " seconds"<< endl;
}


void
finalRegisterSourceToTarget (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        Eigen::Matrix4f &final_transformation_matrix,
        float resolution)
{
    time_t start,end;
    time (&start);

    PointCloud<PointXYZI>::Ptr source_keypoints (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr source_keypoints_registered (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr target_keypoints (new PointCloud<PointXYZI>);

    //uniformKeypoints(source,source_keypoints,resolution); uniformKeypoints(target,target_keypoints,resolution);
    voxelKeypoints(source,source_keypoints,resolution); voxelKeypoints(target,target_keypoints,resolution);

    //agisco sui keypoints, poi applico alla cloud originale
    cout << "SourceKpts: "<<source_keypoints->size()<<" TargetKpts: "<<target_keypoints->size()<<" Final matrix calculation..." << flush;
    //----------------------
    IterativeClosestPoint<PointXYZI, PointXYZI>::Ptr registration (new IterativeClosestPoint<PointXYZI, PointXYZI>);
    //IterativeClosestPointNonLinear<PointXYZI, PointXYZI>::Ptr registration (new IterativeClosestPointNonLinear<PointXYZI, PointXYZI>);
    //----------------------
    registration->setInputCloud(source_keypoints);
    registration->setInputTarget (target_keypoints);
    //registration->setMaxCorrespondenceDistance(50); //4
    registration->setRANSACOutlierRejectionThreshold (10);//0.05 default  Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
    //The method considers a point to be an inlier, if the distance between the target data index and the transformed source index is smaller than the given inlier distance threshold. The value is set by default to 0.05m.
    registration->setRANSACIterations(100);//default 1000
    //----------------------------------------
    //registration->setEuclideanFitnessEpsilon(1);  //1 TermCriterion  The sum of Euclidean squared errors is smaller than a user defined threshold
    registration->setMaximumIterations (100);     //2 TermCriterion  Number of iterations has reached the maximum
    registration->setTransformationEpsilon (1e-7);//3 TermCriterion  The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than parameter
    //----------------------------------------
    registration->align(*source_keypoints_registered);
    final_transformation_matrix = registration->getFinalTransformation();
    if (registration->hasConverged()) cout << " Converged! ";
    cout << "OK! Fitness Score: " << validationScoreProximityTarget<PointXYZI>(*source_keypoints_registered,*target_keypoints)  << endl;
    transformPointCloud(*source, *registered, final_transformation_matrix);

    time (&end);
    double dif = difftime (end,start);
    cout << "------------------------------------->  Final registration in " << dif << " seconds"<< endl;
}

void
finalRegisterSourceToTargetArdoran (
        PointCloud<PointXYZ>::Ptr source ,
        PointCloud<PointXYZ>::Ptr target,
        PointCloud<PointXYZ>::Ptr registered,
        Eigen::Matrix4f &final_transformation_matrix,
        float resolution)
{
    time_t start,end;
    time (&start);

    PointCloud<PointXYZI>::Ptr source_keypoints (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr computing_keypoints (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr source_keypoints_registered (new PointCloud<PointXYZI>);
    PointCloud<PointXYZI>::Ptr target_keypoints (new PointCloud<PointXYZI>);

    if(bool setTrueIfUsingBiggestCluster = true)resolution=0.7;
    //uniformKeypoints(source,source_keypoints,resolution); uniformKeypoints(target,target_keypoints,resolution);
    voxelKeypoints(source,source_keypoints,resolution); voxelKeypoints(target,target_keypoints,resolution);
    //------------------
    //*computing_keypoints=*source_keypoints; //NO PROCESSING ON KEYPOINTS
    //int meanK=5; double stdDev=1; removeOutliers(source_keypoints,computing_keypoints,meanK,stdDev);//OUTLIERS REMOVAL

    PointCloud<PointXYZI>::Ptr new_source_keypoints(new PointCloud<PointXYZI>);//scontorno un po'
    PointCloud<PointXYZI>::Ptr new_target_keypoints(new PointCloud<PointXYZI>);//scontorno un po'
    for(int i=0;i<source_keypoints->size();i++)
        if(source_keypoints->at(i).z<400 && source_keypoints->at(i).z>380 && source_keypoints->at(i).y>160) new_source_keypoints->push_back(source_keypoints->at(i));
    keepBiggestCluster(new_source_keypoints,computing_keypoints,1.5,2000,100000);//USING ONE PARTICULAR CLUSTER
    for(int i=0;i<target_keypoints->size();i++)
        if(target_keypoints->at(i).z<400 && target_keypoints->at(i).z>380 && target_keypoints->at(i).y>160) new_target_keypoints->push_back(target_keypoints->at(i));
    keepBiggestCluster(new_target_keypoints,new_target_keypoints,1.5,2000,100000);//USING ONE PARTICULAR CLUSTER
    //-----------------
    //agisco sui keypoints, poi applico alla cloud originale
    cout << "ComputingKpts: "<<computing_keypoints->size()<<" TargetKpts: "<<new_target_keypoints->size()<<" Final matrix calculation..." << flush;
    //----------------------
    IterativeClosestPoint<PointXYZI, PointXYZI>::Ptr registration (new IterativeClosestPoint<PointXYZI, PointXYZI>);
    //IterativeClosestPointNonLinear<PointXYZI, PointXYZI>::Ptr registration (new IterativeClosestPointNonLinear<PointXYZI, PointXYZI>);
    //----------------------
    registration->setInputCloud(computing_keypoints);
    registration->setInputTarget (new_target_keypoints);
    //registration->setMaxCorrespondenceDistance(50); //4
    registration->setRANSACOutlierRejectionThreshold (10);//0.05 default  Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
    //The method considers a point to be an inlier, if the distance between the target data index and the transformed source index is smaller than the given inlier distance threshold. The value is set by default to 0.05m.
    registration->setRANSACIterations(100);//default 1000
    //----------------------------------------
    //registration->setEuclideanFitnessEpsilon(1);  //1 TermCriterion  The sum of Euclidean squared errors is smaller than a user defined threshold
    registration->setMaximumIterations (100);     //2 TermCriterion  Number of iterations has reached the maximum
    registration->setTransformationEpsilon (1e-7);//3 TermCriterion  The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than parameter
    //----------------------------------------
    registration->align(*source_keypoints_registered);
    final_transformation_matrix = registration->getFinalTransformation();
    transformPointCloud(*source_keypoints, *source_keypoints_registered, final_transformation_matrix);
    if (registration->hasConverged()) cout << " Converged! ";
    cout << "OK! Fitness Score: " << validationScoreProximityTarget<PointXYZI>(*source_keypoints_registered,*target_keypoints)  << endl;
    transformPointCloud(*source, *registered, final_transformation_matrix);

    time (&end);
    double dif = difftime (end,start);
    cout << "------------------------------------->  Final registration in " << dif << " seconds"<< endl;
}


void
registerSourceToTarget (
        PointCloud<PointXYZRGB>::Ptr sourceColor ,
        PointCloud<PointXYZRGB>::Ptr targetColor,
        PointCloud<PointXYZRGB>::Ptr registeredColor)
{
    time_t start,end;
    time (&start);


    //CANCELLA WARNING PCL
    //console::setVerbosityLevel(console::L_ALWAYS);

    PointCloud<PointXYZ>::Ptr source(new PointCloud<PointXYZ>);
    PointCloud<PointXYZ>::Ptr target(new PointCloud<PointXYZ>);

    copyPointCloud(*sourceColor,*source);
    copyPointCloud(*targetColor,*target);
    float voxelResolution=0.5;//<-------------------------        RESOLUTION

    voxel (source,voxelResolution);
    segmentMainPlane(source,4);
    removeOutliers(source,source,2,1);
    voxel (target,voxelResolution);
    segmentMainPlane(target,4);
    removeOutliers(target,target,2,1);



    PointCloud<PointXYZ>::Ptr initialRegistered(new PointCloud<PointXYZ>);
    PointCloud<PointXYZ>::Ptr registered(new PointCloud<PointXYZ>);
    PointCloud<PointXYZI>::Ptr source_keypoints (new PointCloud<PointXYZI> );
    PointCloud<PointXYZI>::Ptr target_keypoints (new PointCloud<PointXYZI> );
    CorrespondencesPtr correspond(new Correspondences);



    //REGISTRATION
    float leafSize=computeLeafSize(target,5000);
    float initialResolution=leafSize*1.5;
    float finalResolution=leafSize*1;
    cout << "Initial Leaf Size: " << initialResolution << " - Final Leaf Size: " << finalResolution<<endl;

    Eigen::Matrix4f initial_transformation_matrix = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f final_transformation_matrix = Eigen::Matrix4f::Identity();
    initialRegisterSourceToTargetEdmond(source,target,initialRegistered,source_keypoints,target_keypoints,correspond,initial_transformation_matrix,initialResolution);
    transformPointCloud(*sourceColor,*registeredColor,initial_transformation_matrix);


    //finalRegisterSourceToTarget(initialRegistered,target,registered,final_transformation_matrix,finalResolution);
    finalRegisterSourceToTargetArdoran(initialRegistered,target,registered,final_transformation_matrix,finalResolution);//no source, devo partire da quella iniziale
    transformPointCloud(*registeredColor,*registeredColor,final_transformation_matrix);


    //FIND CLUSTERS
    /*
    cout << "Clustering..." << flush;
    vector<PointIndices> indices =findClusters(registered,voxelResolution*3,500,30000);
    //inserisce solo i punti dei piani trovati nella cloud newSource
    PointCloud<PointXYZ>::Ptr newRegistered (new PointCloud<PointXYZ>);
    for (int i=0;i<indices.size();i++)//per ogni piano
        for (int j=0;j<indices.at(i).indices.size();j++) //per ogni punto del piano
            newRegistered->push_back(registered->at(indices.at(i).indices.at(j)));
    *registered=*newRegistered;
    cout << "OK! Registered points: " << registered->size() << flush;

    vector<PointIndices> indices2 =findClusters(target,voxelResolution*3,500,30000);
    //inserisce solo i punti dei piani trovati nella cloud newSource
    PointCloud<PointXYZ>::Ptr newTarget (new PointCloud<PointXYZ>);
    for (int i=0;i<indices2.size();i++)//per ogni piano
        for (int j=0;j<indices2.at(i).indices.size();j++) //per ogni punto del piano
            newTarget->push_back(target->at(indices2.at(i).indices.at(j)));
    *target=*newTarget;
    cout << " - OK! Target points: " << target->size() << endl;
    */



    //SEGMENT DIFFERENCES
    /*
    cout << "Differences segmentation..." << endl;
    PointCloud<PointXYZ>::Ptr differ(new PointCloud<PointXYZ>);
    segmentDifferences(registered,target,differ,2.0);

    visualization::PointCloudColorHandlerCustom<PointXYZ> rgb7(differ,255,0,0);
    viewer.addPointCloud<PointXYZ> (differ, rgb7, "differ",v2);
    */

    time (&end);
    double dif = difftime (end,start);
    cout << "------------------------------------->  Total registration in " << dif << " seconds"<< endl;
}

